/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.iaroc.irobot.sample;

import com.iaroc.irobot.*;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.util.Utils;

/**
 *
 * @author Mark
 */
public class FinalHomeBaseEventHandler extends IRobotCreateEventHandler 
    {
    //Data Members
    protected IRobotCreate create;
    protected EDemoBoard board;
    protected byte[] lastDetected;
    protected byte state;
    protected boolean signal;
    // constants
    public final byte DEAD_VALUE = -1;
    public final byte RED_BUOY = 1;
    public final byte GREEN_BUOY = 2;
    public final byte FORCE_FIELD = 3;
    public final byte RED_FORCE = 4;
    public final byte GREEN_FORCE = 5;
    public final byte RED_GREEN_BUOY = 6;
    public final byte RED_GREEN_FORCE = 7;
    
    public FinalHomeBaseEventHandler(IRobotCreate create, EDemoBoard board)
        {
        super();
        this.create = create;
        this.board = board;
        create.setVelocity(300);
        lastDetected = new byte[3];
        for (byte i = 0; i < lastDetected.length; i++)
            lastDetected[i] = 0;
        System.out.println("\nFinal Start");
        signal = false;
        }
    
    protected void updateLast(int bit)
        {
        byte[] next = new byte[lastDetected.length];
        for (byte i = 1, x = 0; i < lastDetected.length; i++, x++)
            next[x] = lastDetected[i];
        byte last = 0;
        switch (bit)
            {
            case 255:
            case 240:
                last = DEAD_VALUE;
                break;
            case 248:
                last = RED_BUOY;
                break;
            case 244:
                last = GREEN_BUOY;
                break;
            case 242:
                last = FORCE_FIELD;
                break;
            case 252:
                last = RED_GREEN_BUOY;
                break;
            case 250:
                last = RED_FORCE;
                break;
            case 246:
                last = GREEN_FORCE;
                break;
            case 254:
                last = RED_GREEN_FORCE;
                break;
            default:
                throw new IllegalArgumentException("Bad byte Value: " + bit);
            }
        next[next.length - 1] = last;
        lastDetected = next;
        }
    
    public void runHome()
        {
        while (true)
            {
            pollSensors();
            Utils.sleep(100);
            }
        }
    
    protected void pollSensors() 
        {
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, this);
        }

    public void eventBatchEnd() 
        {
        super.eventBatchEnd();
        if (!signal)
            {
//            create.goForward();
            System.out.println("No byte found, going forward");
            }
        else
            create.stop();
        create.demo(IRobotCreateConstants.DEMO_HOME);
        evaluateReading();
        }

    public void eventBatchStart() 
        {
        super.eventBatchStart();
        signal = false;
        }

    public void infraredByteEvent(int oldInt, int infraredByte) 
        {
        super.infraredByteEvent(oldInt, infraredByte);
        signal = true;
        updateLast(infraredByte);
        System.out.println("Infrared Byte : " + infraredByte);
        }
    
    protected void evaluateReading()
        {
        if (lastDetected[2] == 255)
            create.drive(create.getVelocity(), -75);
        
        }
    }//class FinalHomeBaseEventHandler